/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-21     huger       the first version
 */


#include "bldc_motor.h"
#include "Foc/foc.h"
#include "Pid/pid.h"
#include <IMU/Imu.h>
#include "bsp/bsp_adc.h"
#include "bsp/bsp_spi2.h"
#include "Magnetic/magnetic.h"

#define DBG_TAG "BLDC"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>


uint32_t hPhaseA_OffSet=0;
uint32_t hPhaseB_OffSet=0;
uint32_t hPhaseC_OffSet=0;

float Speed_Ref =50.0F;
float Speed_Pid_Out;
float Speed_Fdk;

float Posi_Ref = 0.0;
float Posi_Pid_Out;
float Posi_Fdk;

uint8_t State=STOP;  //运行状态
uint8_t Motor_Run_Mode = pos;

static Magnetic t_magnetic;


static float_t imu_value[3]={0};    //imu数值
static float_t imu_roll_offset=0.0; //imu偏移
static float_t imu_roll_preview=0.0; //imu偏移
static float encoder_offset;        //编码器偏移

float_t magnetic_current_pos=0;  //当前位置
//static  float_t magnetic_current_pos=0;  //当前位置
static  float_t magnetic_current_spd=0; //当前速度

/* 指向信号量的指针 */
static rt_sem_t foc_clc_sem = RT_NULL;  //信号量
static uint8_t adc_inject_count=0;//adc采集中断次数累计

static int16_t motor_spd=0;
static int32_t motor_pos=0;

static void motor_pid_clc(void)
{
    magnetic_current_pos = t_magnetic.magnetic_encoder_degree_get()/360.0*cpr;
    magnetic_current_spd = t_magnetic.magnetic_encoder_velocity_clc(PID_SAMPLE_TIME)/360.0*cpr;
    if(State==RUN)
    {
        if(Motor_Run_Mode == spd)
        {
            Speed_Pid_Calc(Speed_Ref,magnetic_current_spd,&Speed_Pid_Out,&Speed_Pid);
            motor_spd = magnetic_current_spd;

        }else if(Motor_Run_Mode == pos)
        {
            get_imu_angel(imu_value);//读取imu数值
//            Posi_Ref = (imu_roll_offset - imu_value[0])/360.0*cpr;  //imu偏置多少,电机需要往回补偿多少



            Posi_Ref = Posi_Ref + (imu_raw_angle_clc(imu_roll_preview,imu_value[0])/360*cpr);
//            rt_kprintf("i:%f,c:5f,s:%f\n",initial_angle,initial_angle,sum);
            imu_roll_preview = imu_value[0];


            Posi_Pid_Calc(Posi_Ref,magnetic_current_pos,&Posi_Pid_Out,&Posi_Pid);  //位置pid计算目标速度
            motor_pos = magnetic_current_pos;
            float_t target_spd = Posi_Pid_Out*SPEED_SAMPLE_FREQ;  //速度转换

            if(target_spd > Speed_Ref)target_spd = Speed_Ref;//限幅
            else if(target_spd < -Speed_Ref){ target_spd = -Speed_Ref ;}   //限幅

            Speed_Pid_Calc(target_spd,magnetic_current_spd,&Speed_Pid_Out,&Speed_Pid);   //速度 pid计算
            motor_spd = magnetic_current_spd;
        }
    }
}

/* 线程入口 */
static void Clc_Magangel(void* parameter)
{
    static rt_err_t result;
    while (1)
    {
        result = rt_sem_take(foc_clc_sem, RT_WAITING_FOREVER);
        if(result == RT_EOK)
        {
            motor_pid_clc();
            if(State==RUN)foc_algorithm_step();
        }
    }
}


void Motor_control_thread_init(void)
{
    rt_thread_t tid = RT_NULL;

    /* 创建一个动态信号量，初始值是 0 */
    foc_clc_sem = rt_sem_create("foc_clc", 0, RT_IPC_FLAG_PRIO);
    if (foc_clc_sem == RT_NULL)
    {
        rt_kprintf("create foc_clc semaphore failed.\n");
    }
    else
    {
        rt_kprintf("create foc_clc semaphore sucess.\n");
    }

    /* 创建线程 1 */
    tid = rt_thread_create("clc_angel",
                            Clc_Magangel, RT_NULL,
                            1024,
                            15, 20);
    if (tid != RT_NULL)
        rt_thread_startup(tid);
}


/**********************************************************************************************************
get Theta
**********************************************************************************************************/
float_t ENC_Get_Electrical_Angle(void)
{
    float_t temp;
    temp= magnetic_current_pos  * POLE_PAIR_NUM ;
    return fmod(temp,cpr);
}

void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc)//ADC回调函数
{
    if(adc_inject_count<foc_cyclic_time)adc_inject_count++;
    else {
        adc_inject_count=0;
        rt_sem_release(foc_clc_sem);
    }
}


/**********************************************************************************************************
get Iab
**********************************************************************************************************/
CURRENT_ABC_DEF get_Iab(void)
{
    CURRENT_ABC_DEF Iab_temp;
    Iab_temp.Ia =((int16_t)((int16_t)hPhaseA_OffSet)-(int16_t)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)) * SAMPLE_CURR_CON_FACTOR;
    Iab_temp.Ib =((int16_t)((int16_t)hPhaseB_OffSet)-(int16_t)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)) * SAMPLE_CURR_CON_FACTOR;
    Iab_temp.Ic = -Iab_temp.Ia - Iab_temp.Ib;
    return (Iab_temp);
}


/*FOC Core part 15Khz*/
void foc_algorithm_step(void)
{
    float_t temp = ENC_Get_Electrical_Angle();
    Clarke_Transf(get_Iab(),&Current_Ialpha_beta);
    float_t theta = (temp - encoder_offset) > 0 ? cpr -(temp - encoder_offset) : encoder_offset - temp;
    Angle_To_Cos_Sin(theta,&Transf_Cos_Sin); //when use openloop, angle=cnt;ENC_Get_Electrical_Angle()-encoder_offset
    Park_Transf(Current_Ialpha_beta,Transf_Cos_Sin,&Current_Idq);

//  /*PID Control part*/
    IQ_REF = Speed_Pid_Out;
    ID_REF = 0.0F;
    Current_PID_Calc(ID_REF,Current_Idq.Id,&Voltage_DQ.Vd,&Current_D_PID);
    Current_PID_Calc(IQ_REF,Current_Idq.Iq,&Voltage_DQ.Vq,&Current_Q_PID);

//    Voltage_DQ.Vd = 0; Voltage_DQ.Vq=-4;
    Rev_Park_Transf(Voltage_DQ,Transf_Cos_Sin,&Voltage_Alpha_Beta);
    SVPWM_Calc(Voltage_Alpha_Beta,Udc,PWM_TIM_PULSE_TPWM);

}
void Start_Up(void)
{
    encoder_offset = nor_flash_float_read(easyflash_add);

    if(encoder_offset == 0xffffffff)
    {
        rt_kprintf("----motor hasn't be calibrated ,motor calibrate begin----\r\n");

        Voltage_DQ.Vd=Start_V;
        Voltage_DQ.Vq=0.0f;
        Angle_To_Cos_Sin(0.0,&Transf_Cos_Sin);
        Rev_Park_Transf(Voltage_DQ,Transf_Cos_Sin,&Voltage_Alpha_Beta);
        SVPWM_Calc(Voltage_Alpha_Beta,Udc,PWM_TIM_PULSE_TPWM);
        rt_thread_mdelay(2000);
        encoder_offset = ENC_Get_Electrical_Angle();   //encoder_offset  -----转子与a周平行时编码器的数值       --- theta=0

        nor_flash_write(easyflash_add,encoder_offset);

        Posi_Ref = magnetic_current_pos;

        get_imu_angel(imu_value);
//        imu_roll_offset = imu_value[0]+(Posi_Ref*360/cpr);
        imu_roll_preview = imu_value[0];

        Posi_Ref = Posi_Ref + (imu_raw_angle_clc(imu_roll_preview,imu_value[0])/360*cpr);
        imu_roll_preview = imu_value[0];

        rt_kprintf("----motor calibrate over,encoder_offset=%f----\n",encoder_offset);
    }
    else{
        rt_thread_mdelay(100);
        Posi_Ref = magnetic_current_pos;
        get_imu_angel(imu_value);
//        imu_roll_offset = imu_value[0]+(Posi_Ref*360/cpr);

        imu_roll_preview = imu_value[0];

        Posi_Ref = Posi_Ref + (imu_raw_angle_clc(imu_roll_preview,imu_value[0])/360*cpr);
        imu_roll_preview = imu_value[0];

        LOG_D("motor has be calibrated ,encoder offset read ok,encoder_offset=%f\n",encoder_offset);
    }
    State=RUN;
}

static int Get_Imu_MotorAngle(int argc, char *argv[])
{
    static float_t imu_value[3]={0};
    get_imu_angel(imu_value);
    float_t temp = magnetic_current_pos;
    rt_kprintf("ele= %.3f, angle = %.3f, r=%.3f, p=%.3f, y=%.3f\n",t_magnetic.correct_degree,temp,imu_value[0],imu_value[1],imu_value[2]);
    return RT_EOK;
}
MSH_CMD_EXPORT(Get_Imu_MotorAngle, get imu_value and motor_angel);




